package javavis.jip3d.functions;

import java.util.ArrayList;

import javavis.base.Function3DGroup;
import javavis.base.JIPException;
import javavis.base.ParamType;
import javavis.jip3d.geom.MyTransform;
import javavis.jip3d.geom.MyTransform2D;
import javavis.jip3d.geom.MyTransform3D;
import javavis.jip3d.gui.Function3D;
import javavis.jip3d.gui.FunctionParam;
import javavis.jip3d.gui.ScreenData;
import javavis.jip3d.gui.ScreenOptions;
import javavis.jip3d.gui.dataobjects.PlaneSet3D;
import javavis.jip3d.gui.dataobjects.Trajectory;
import javavis.jip3d.gui.dataobjects.Trajectory2D;
import javavis.jip3d.gui.dataobjects.Trajectory3D;

public class FProcessEgomotion extends Function3D {

	public FProcessEgomotion() {
		super();
		this.allowed_input = ScreenOptions.tTRAJ2D | ScreenOptions.tTRAJ3D;
		this.group = Function3DGroup.Egomotion;


		FunctionParam p1 = new FunctionParam("Name", ParamType.STRING);
		p1.setValue("Egomotion");
		FunctionParam p2 = new FunctionParam("3DOF", ParamType.BOOL);
		p2.setValue(false);
		FunctionParam p3 = new FunctionParam("Alpha", ParamType.FLOAT);
		p3.setValue(0.15); //0.0125

		FunctionParam p4 = new FunctionParam("Ground Truth", ParamType.BOOL);
		p4.setValue(false);

		FunctionParam p5 = new FunctionParam("Step", ParamType.INT);
		p5.setValue(1);

		this.addParam(p1);
		this.addParam(p3);
		this.addParam(p2);
		this.addParam(p4);
		this.addParam(p5);
	}

	@Override
	public void proccessData(ScreenData scr_data) throws JIPException {
		result_list = new ArrayList<ScreenData>();

		String output_name = this.paramValueString("Name");
		boolean three_dof = this.paramValueBool("3DOF");
		Trajectory traj = (Trajectory)scr_data;
		int cont, tam;
		PlaneSet3D old_planes, planes;
		String file_name;
		double alpha = this.paramValueReal("Alpha");
		int step = this.paramValueInt("Step");

		boolean groundTruth = paramValueBool("Ground Truth");
		MyTransform gt_trans, new_trans, total_trans;
		double errX, errY, errZ, errAX, errAY, errAZ;
		double errorPos, errorPosTotal=0;
		double errorAng, errorAngTotal=0;

		long total_time, t1, t2;
		int numPoses = 0;

		Function3D egomotion;
		Trajectory new_traj;
		if(three_dof)
		{
			egomotion = new FEgomotion2D();
			new_traj = new Trajectory2D(new ScreenOptions(),0);
		}
		else
		{
//			egomotion = new FEgomotion3D();
			egomotion = new FFastEgomotion3D();
//			egomotion.setParamValue("Selected", 0.5);
			egomotion.setParamValue("Alpha", alpha);
			if(groundTruth) egomotion.setParamValue("Verbose", false);
			new_traj = new Trajectory3D(new ScreenOptions());
		}
		new_traj.name = output_name;
		new_traj.path = traj.path;

		tam = traj.files.size();
		total_time = 0;

		double prog_inc = 100.0 / tam;

		file_name = traj.files.get(0);
		old_planes = new PlaneSet3D(new ScreenOptions());
		old_planes.readData(file_name, traj.path);
		new_traj.files.add(file_name);
		if(three_dof)
			new_traj.transforms.add(new MyTransform2D());
		else
			new_traj.transforms.add(new MyTransform3D());
		total_trans = new MyTransform3D();
		for(cont=1;cont<tam;cont++)
		{
			if(!groundTruth) System.out.println("image "+cont);
			file_name = traj.files.get(cont);
			gt_trans = traj.transforms.get(cont);
			total_trans.applyTransform(gt_trans);
			
			if(cont%step==0)
			{
				numPoses++;
				planes = new PlaneSet3D(new ScreenOptions());
				planes.readData(file_name, traj.path);
				if(three_dof)
					//TODO apañar el tema modelo/escena en FEgomotion2D
					egomotion.setParamValue("Next Object", planes);
				else
					egomotion.setParamValue("Model", planes);
				t1 = System.currentTimeMillis();
				egomotion.proccessData(old_planes);
				t2 = System.currentTimeMillis();
				total_time += t2 - t1;

				new_traj.files.add(file_name);
				if(three_dof)
					new_traj.transforms.add(((FEgomotion2D)egomotion).getTransform());
				else
				{
					new_trans = ((FFastEgomotion3D)egomotion).getTransform();
//					new_trans = ((FEgomotion3D)egomotion).getTransform();
					new_traj.transforms.add(new_trans);
					if(groundTruth)
					{
						errX = total_trans.trX - new_trans.trX;
						errY = total_trans.trY - new_trans.trY;
						errZ = total_trans.trZ - new_trans.trZ;
						errAX = total_trans.angX - new_trans.angX;
						errAY = total_trans.angY - new_trans.angY;
						errAZ = total_trans.angZ - new_trans.angZ;
						errorPos = errX*errX + errY*errY + errZ*errZ;
						errorPosTotal += errorPos;
						errorPos = Math.sqrt(errorPos);
						errorAng = errAX*errAX+errAY*errAY+errAZ*errAZ;
						errorAngTotal += errorAng;
						errorAng = Math.sqrt(errorAng);
						System.out.println(cont+": Pos: "+errorPos+" Ang: "+errorAng);
					}
				}
				total_trans = new MyTransform3D();
				old_planes = planes;
			}
			progress += prog_inc;
		}

		if(groundTruth)
		{
			System.out.println("Pose RMS: "+Math.sqrt(errorPosTotal/numPoses));
			System.out.println("Ang. RMS: "+Math.sqrt(errorAngTotal/numPoses));
		}
		System.out.println("\nEgomotion Time (mean): "+total_time/(tam/step)+" ms");

System.out.println("Tam: "+tam+" step: "+ step+" total time: "+total_time);
		new_traj.scr_opt.num_points = numPoses;
		new_traj.writeData(output_name, traj.path);
		this.result_list.add(new_traj);

	}

}
